#include <stdio.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <netinet/in.h>
#include <iostream>

#include "ExceptionUtil.hpp"
#include "ErrnoException.hpp"

#include "messages/FindDevicesReply.hpp"
#include "GetAccelReadingReply.hpp"

#include "NetUtil.hpp"
#include "IncomingPacket.hpp"
#include "Log.hpp"

#define BUFFSIZE 255
void Die(char *mess) {
    perror(mess); exit(1);
}

uint16_t LISTEN_PORT = 22222;
uint16_t DEVICE_ID = 111;
static const char* DEVICE_DESC = "3-axis accelerometer";

unsigned int server_port;

using namespace kieper;

int main(int argc, char *argv[]) {
    int sock;
    int clientSock;
    struct sockaddr_in echoserver;
    struct sockaddr_in echoclient;
    struct sockaddr_in clientAddr;

    char buffer[BUFFSIZE];
    socklen_t echolen, clientlen, serverlen;
    int received = 0;

    /* Create the UDP socket */
    if ((sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
        Die("Failed to create socket");
    }

    /* Construct the server sockaddr_in structure */
    memset(&echoserver, 0, sizeof(echoserver));       /* Clear struct */
    echoserver.sin_family = AF_INET;                  /* Internet/IP */
    echoserver.sin_addr.s_addr = htonl(INADDR_ANY);   /* Any IP address */
    echoserver.sin_port = htons(22222);       /* server port */

    /* Bind the socket */
    serverlen = sizeof(echoserver);
    if (bind(sock, (struct sockaddr *) &echoserver, serverlen) < 0) {
        Die("Failed to bind server socket");
    }
    
    try {
        /* Run until cancelled */
        while (1) {
            /* Receive a message from the client */
            clientlen = sizeof(echoclient);
            if ((received = recvfrom(sock, buffer, BUFFSIZE, 0,
                            (struct sockaddr *) &echoclient,
                            &clientlen)) < 0) {
                Die("Failed to receive message");
            }
            fprintf(stderr,
                    "Client connected: %s\n", inet_ntoa(echoclient.sin_addr));
            /* Send the message back to client */
            
            IncomingPacket packet(buffer, received - Packet::HEADER_SIZE);
                LOG_TRACE("Received packet id=%u devId = %u type = %d, del = %d, scope = %d, dir = %d, \
                        msgnum = %u", packet.getPacketId(), packet.getDeviceId(), packet.getType(), packet.getDelivery(), packet.getScope(),
                        packet.getDirection(), packet.getMessageNumber());
            if (packet.getDeviceId() == 0 &&
                packet.getType() == Packet::TYPE_CONTROL &&
                packet.getDelivery() == Packet::DELIVERY_ASYNC &&
                packet.getScope() == Packet::SCOPE_GLOBAL &&
                packet.getDirection() == Packet::DIRECTION_REQUEST &&
                packet.getMessageNumber() == 0) {

                LOG_DEBUG("Received proper packet", 1);
                server_port = netutil::getFromNbo16(packet.getPayload(), 0);
                LOG_DEBUG("Port = %u", server_port);
                
                memset(&clientAddr, 0, sizeof(clientAddr));
                clientAddr.sin_family = AF_INET;
                //clientAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
                clientAddr.sin_addr = echoclient.sin_addr;
                clientAddr.sin_port = htons(server_port);

                clientSock = netutil::createUdpSock();
                FindDevicesReply msg(DEVICE_ID, LISTEN_PORT, DEVICE_DESC);
                
                int sendSize = msg.getPacket().getTotalSize();

                LOG_TRACE("Waiting 100mS",0);
                usleep(100000);
                LOG_TRACE("Sending packet of size %d", sendSize);
                if (sendto(clientSock, msg.getPacket().getBuffer(), sendSize, 0, 
                            reinterpret_cast<sockaddr*> (&clientAddr), sizeof(clientAddr)) != sendSize) {
                    THROW(ErrnoException, "Failed to send FindDeviceMsg packet");
                }
                LOG_TRACE("Sent packet",0);

            } else if (packet.getDeviceId() == 0 &&
                packet.getType() == Packet::TYPE_DATA &&
                packet.getDelivery() == Packet::DELIVERY_SYNC &&
                packet.getScope() == Packet::SCOPE_DEVICE_SPECIFIC &&
                packet.getDirection() == Packet::DIRECTION_REQUEST &&
                packet.getMessageNumber() == 0) {
                GetAccelReadingReply msg(packet.getPacketId(), 0, 10, 20, 30); 
                
                int sendSize = msg.getPacket().getTotalSize();
                LOG_TRACE("Sending packet of size %d", sendSize);
                if (sendto(clientSock, msg.getPacket().getBuffer(), sendSize, 0, 
                            reinterpret_cast<sockaddr*> (&clientAddr), sizeof(clientAddr)) != sendSize) {
                    THROW(ErrnoException, "Failed to send accel reply packet");
                }
                LOG_TRACE("Sent packet",0);
            }
        }
    } catch (KieperException& e) {
        std::cout << "Exception caught!" << std::endl;
        std::cout << e.longMsg() << std::endl;
    }
}

